﻿#ifndef CALADIST_TOOL_H
#define CALADIST_TOOL_H
#include "Object.h"
/******************************************************************************
  @Project Name  : 03TestFCLCollision
  @File Name     : CalaDist_Tool.h
  @Version       : 1.0
  @Function      : to be determined
  @Author        : unishuai
  @Date          : 2024-05-20
******************************************************************************/

typedef struct DistMsg
{
    double distance;
    std::string objA;
    std::string objB;
    Eigen::Vector3d pointA;
    Eigen::Vector3d pointB;

public:
    //    这个主要是只做了一个初始化的工作
    DistMsg()
    {
        distance=-1;
        objA="NULL";
        objB="NULL";
        pointA[0]=0;pointA[1]=0;pointA[2]=0;
        pointB[0]=0;pointB[1]=0;pointB[2]=0;
    }
    DistMsg(double distance,std::string &objA,std::string &objB,Eigen::Vector3d &pointA,Eigen::Vector3d &pointB)
    {
        this->distance=distance;
        this->objA=objA;
        this->objB=objB;
        this->pointA[0]=pointA[0];
        this->pointA[1]=pointA[1];
        this->pointA[2]=pointA[2];
        this->pointB[0]=pointB[0];
        this->pointB[1]=pointB[1];
        this->pointB[2]=pointB[2];
    }
    DistMsg(double distance, std::string objA, std::string objB,std::vector<double> pointA, std::vector<double> pointB)
    {
        this->distance=distance;
        this->objA=objA;
        this->objB=objB;
        this->pointA[0]=pointA[0];
        this->pointA[1]=pointA[1];
        this->pointA[2]=pointA[2];
        this->pointB[0]=pointB[0];
        this->pointB[1]=pointB[1];
        this->pointB[2]=pointB[2];
    }
    // 输出操作符重载
    friend std::ostream& operator<<(std::ostream& os, const DistMsg& msg) {
        os << std::fixed << std::setprecision(3);
        os << "Distance: " << msg.distance << "\n";
        os << "Object A: " << msg.objA << "\n";
        os << "Object B: " << msg.objB << "\n";
        os << "Point A: (" << msg.pointA[0] << ", " << msg.pointA[1] << ", " << msg.pointA[2] << ")\n";
        os << "Point B: (" << msg.pointB[0] << ", " << msg.pointB[1] << ", " << msg.pointB[2] << ")\n";
        return os;
    }
    double getDistance() const;
    void setDistance(double value);
    std::string getObjA() const;
    void setObjA(const std::string &value);
    std::string getObjB() const;
    void setObjB(const std::string &value);
    Eigen::Vector3d getPointA() const;
    void setPointA(const Eigen::Vector3d &value);
    Eigen::Vector3d getPointB() const;
    void setPointB(const Eigen::Vector3d &value);
}DistMsg;



//这个相当于是一个仿函数，允许像函数一样调用
class CompareDistMsg
{
public:
    bool operator()(const DistMsg &msg1,const DistMsg &msg2)
    {
        if(msg1.distance<msg2.distance){return true;}
        else{return false;}
    }
};



class CalcDist_Tool
{
public:

    CalcDist_Tool();
    ~CalcDist_Tool() = default;
    //单例模式
    static CalcDist_Tool& Instance();
    bool m_IsInit;

    //类对象初始化
    bool Init(void);

    std::vector<RobotObj> RobotObjs;
    std::vector<TunnelWallObj> TunnelWallsObjs;
    std::vector<MotionPlatformObj> MotionPlatformObjs;
    std::vector<RadarObj> RadarObjs;
    std::vector<FlightObj> FlightObjs;
    void initMod(QString path);




    std::array<double,5> checkArm2Self(int arm_index,int base_index,std::array<double,5> angle,int check_index);

    std::array<double,5> checkArm2TunnelWall(int arm_index,int base_index,std::array<double,5> angle,int platform_index);
    std::array<double,5> checkArm2TunnelWallWithPose(int arm_index,int base_index,std::array<double,5> angle,int platform_index,std::array<double,6>  PlatformPose);
    std::array<double,5> checkArmWithSlider2TunnelWallWithPose(int arm_index,int base_index,std::array<double,5> posAndAngle,int platform_index,std::array<double,6>  PlatformPose);
    std::array<double,6> checkArm2TunnelWall(int arm_index,int base_index,std::array<double,6> angle,int platform_index);
    std::array<double,6> checkArm2SW(int arm_index,int base_index,std::array<double,6> angle,int cabin_index,double s_angle);
    std::array<double,7> checkCombinedArm2TunnelWall(int arm_index,Eigen::Matrix4d T_inst,std::array<double,6> angle,int cabin_index);
    std::array<double,13> checkCombinedArms2TunnelWall(int arm_index,int base_index,std::array<double,6> angle,
                                                       int arm_index2,Eigen::Matrix4d T_inst,std::array<double,6> angle2,int cabin_index);
    std::array<double,7> checkCombinedArm2SW(int arm_index,Eigen::Matrix4d T_inst,std::array<double,6> angle,int cabin_index,double s_angle);
    std::array<double,13> checkCombinedArms2SW(int arm_index,int base_index,std::array<double,6> angle,
                                               int arm_index2,Eigen::Matrix4d T_inst,std::array<double,6> angle2,int cabin_index,double s_angle);
    std::vector<DistMsg> checkArm2Radar(int arm_index,int base_index,std::array<double,6> angle,int radar_index,Eigen::VectorXd angle2);

    /**=================by unishuai====================
    *下面是添加的飞机的碰撞函数
    *===================2024-05-04 ====================*/



    [[deprecated ("这个类过时了，没有用过，没有修改")]]
    std::vector<DistMsg> checkTunnelWallWithPose2FlightWithAngle(int form_index,std::array<double,6>  PlatformPose,int flight_index,std::array<double,3> flightAngle);

    /**=================by unishuai====================
    *todo:1 连杆机构和飞机的碰撞检测
    *===================2024-05-19 ====================*/
    std::vector<DistMsg> checkArmWithSlider2FlightWithAngle(int arm_index,int base_index,std::array<double,5> angle,int flight_index,std::array<double,3> flightAngle);
    /**=================by unishuai====================
    *todo:3 动平台机构和飞机的碰撞检测
    *===================2024-05-19 ====================*/
    std::vector<DistMsg> checkMotionPlatformWithPose2FlightWithAngle(int motionform_index,std::array<double,6>  PlatformPose,std::array<double,1> missileAngle,int flight_index,std::array<double,3> flightAngle);
    /**=================by unishuai====================
    *todo:2 已经更改成为了动平台机构和静平台(洞壁)的碰撞
    *===================2024-05-19 ====================*/
    std::vector<DistMsg> checkMotionPlatform2TunnelWall(std::array<double,6>  motionPlatformPose,std::array<double,1> missileAngle);
    /**=================by unishuai====================
    *todo:4 检测平台中一边杆的碰撞检测,通过输入起始点和杆的点的数量进行判断
    *===================2024-05-19 ====================*/
    // 这里我先写一个连杆机构和连杆机构的碰撞检测
    // 这里我感觉有些抽象，连杆之间相互检测需要25次
    // 一侧就需要75次
    std::vector<DistMsg> checkArmWithSlider2ArmWithSlider(int armIndexOne,int baseIndexOne,std::vector<double> sliderOne,int armIndexTwo,int baseIndexTwo,std::vector<double> sliderTwo);
    /**
     * @brief checkArmWithSliderBySideNum
     * 这里就是简单的封装了一下这个
     * @param sideNum 0表示第一侧，1表示另外一侧
     * @param angles
     * @return
     */
    std::vector<std::vector<DistMsg>> checkArmWithSliderBySideNum(int sideNum,std::vector<std::vector<double>> angles);



    /**=================by unishuai====================
    *01again: 连杆机构和飞机的碰撞检测(只检测碰撞，不检测距离)
    *===================2024-05-29 ====================*/
    std::vector<DistMsg> checkArmWithSlider2FlightWithAngleJustCol(int arm_index,int base_index,std::array<double,5> SliderAndAngle,int flight_index,std::array<double,3> flightAngle);
    /**=================by unishuai====================
    *02again: 检测动平台机构和洞壁的碰撞(只检测碰撞，不检测距离)
    *===================2024-05-19 ====================*/
    std::vector<DistMsg> checkMotionPlatform2TunnelWallJustCol(std::array<double,6>  motionPlatformPose,std::array<double,1> missileAngle);
    /**=================by unishuai====================
     *03again: 动平台机构和飞机的碰撞检测(只检测碰撞，不检测距离)
     *===================2024-05-19 ====================*/
    std::vector<DistMsg> checkMotionPlatformWithPose2FlightWithAngleJustCol(
            int motionform_index,std::array<double,6>  motionPlatformPose,
            std::array<double,1> missileAngle,int flight_index,std::array<double,3> flightAngle);
    /**=================by unishuai====================
    *04again: 检测平台中一边杆的碰撞检测,
    * 通过输入起始点和杆的点的数量进行判断(只检测碰撞，不检测距离)
    *===================2024-05-19 ====================*/
     std::vector<DistMsg> checkArmWithSlider2ArmWithSliderJustCol(
             int armIndexOne,int baseIndexOne,std::vector<double> sliderOne,
             int armIndexTwo,int baseIndexTwo,std::vector<double> sliderTwo);

     std::vector<std::vector<DistMsg>> checkArmWithSliderBySideNumJustCol(
             int sideNum,std::vector<std::vector<double>> angles);


    //private:

    void readXml(QString path);
    void loadOBJFile(const char* filename, std::vector<fcl::Vector3d>& points, std::vector<fcl::Triangle>& triangles);
};

#endif // CALADIST_TOOL_H
